function main0712
%%main0614�������޸�
clc;clear all;close all;
HiddenUnitNum=7;
rand('state',sum(100*clock));
% ԭʼ���� 
d=[        66,     41.45,        0,        0,       0];
a=[        0,    0,   -82.85,   -82.85,       -73.85];
alpha=[    0,  pi/2,        0,        0,    -pi/2];
L(1)=Link('d',d(1),'a',a(1),'alpha',alpha(1)); 
L(2)=Link('d',d(2),'a',a(2),'alpha',alpha(2));
L(3)=Link('d',d(3),'a',a(3),'alpha',alpha(3));
L(4)=Link('d',d(4),'a',a(4),'alpha',alpha(4));
L(5)=Link('d',d(5),'a',a(5),'alpha',alpha(5));
du=pi/180;
%����ؽڷ�Χ
L(1).qlim =[0,180]*du;
L(2).qlim =[0,180]*du;%-10,130
L(3).qlim =[0,180]*du;%-140,0
L(4).qlim =[0,180]*du;
L(5).qlim =[0,270]*du;
dofbot=SerialLink(L,'name','�����ɶȻ�е��');
Theta=[0 0 0 0 0];
Theta=Theta/180*pi;
Theta2=[pi pi pi pi pi];
Theta2=Theta2/180*pi;   
forward1=dofbot.fkine(Theta)
forward2=dofbot.fkine(Theta2)
T1=forward1;
T2=forward2;
%��Ӧ�ؽڽ�
q1=dofbot.ikine(T1,[0 0 0 0 0],[1 1 1 1 1 0]);
q2=dofbot.ikine(T2,[0 0 0 0 0],[1 1 1 1 1 0]);
%��A�ƶ���B����ʱ6s��ÿ100ms����һ�ιؽڽ�
t=[0:0.1:60]';
%A�㵽B�㣬6���ؽڽ���6s�еı任���̡�v��a���ٶȺͼ��ٶȡ�
[q,v,a]=jtraj(q1,q2,t);
t1=q(:,1);
t2=q(:,2);
t3=q(:,3);
t4=q(:,4);
t5=q(:,5);
%����ѵ������.һ��ȷ��λ�ڹ����ռ��ڡ�
syms theta1 theta2 theta3 theta4 theta5
d1=66;
d2=41.45;
a3=-82.85;
a4=-82.85;
a5=-73.85;
A1=[cos(theta1) -sin(theta1) 0 0;
    sin(theta1) cos(theta1) 0 0;
    0 0 1 d1;
    0 0 0 1];
A2=[cos(theta2) 0 sin(theta2) 0;
    sin(theta2) 0 -cos(theta2) 0;
    0 1 0 d2;
    0 0 0 1];
A3=[cos(theta3) -sin(theta3) 0 a3*cos(theta3);
    sin(theta3) cos(theta3) 0 a3*sin(theta3);
    0 0 1 0;
    0 0 0 1];
A4=[cos(theta4) -sin(theta4) 0 a4*cos(theta4);
    sin(theta4) cos(theta4) 0 a4*sin(theta4);
    0 0 1 0;
    0 0 0 1];
A5=[cos(theta5) 0 -sin(theta5) a5*cos(theta5);
    sin(theta5) 0 cos(theta5) a5*sin(theta5);
    0 -1 0 0;
    0 0 0 1];
T05=simplify(A1*A2*A3*A4*A5);
Theta=[];
px=[];
py=[];
pz=[];
Tx=[];
Ty=[];
Tz=[];
[N,xxx]=size(q);%xxxΪ5
for i=1:N
    theta1=t1(i);
    theta2=t2(i);
    theta3=t3(i);
    theta4=t4(i);
    theta5=t5(i);
    Lie=[theta1
        theta2
        theta3
        theta4
        theta5];
    Theta=[Theta,Lie];
    T=eval(T05);
    px=[px
        T(1,4)];%Ŀ��������������
    py=[py
        T(2,4)];%Ŀ��������������
    pz=[pz
        T(3,4)];%Ŀ��������������
    R=T(1:3,1:3);
    r11=R(1,1);
    r12=R(1,2);
    r13=R(1,3);
    r21=R(2,1);
    r22=R(2,2);
    r23=R(2,3);
    r31=R(3,1);
    r32=R(3,2);
    r33=R(3,3);
    Tz=[Tz
        atan2(r21,r11)];%Ŀ��������������
    Ty=[Ty
        atan2(-r31,sqrt((r32^2)+(r33^2)))];%Ŀ��������������
    Tx=[Tx
        atan2(r32,r33)];%Ŀ��������������
end
                
%�������ݾ���
IN=[px py pz Tx Ty Tz]';
%Ŀ�����ݾ���
OUT=Theta;

% �������ݾ���
p=IN;  %6x601
 % Ŀ�����ݾ���
t=OUT;   %5X601
% ����mapminmax���������ݽ��й�һ��
[pn,input_str] = mapminmax(p) ;
[tn,output_str] = mapminmax(t) ;
TrainSamIn=pn;
TrainSamOut=tn;
[xxx1,TrainSamNum]=size(TrainSamIn);%xxx1Ϊ6�����������ά�ȣ��˶�ѧ�����е�δ֪����
[xxx2,TrainSamNum]=size(TrainSamOut);%xxx2Ϊ5�����������ά�ȣ�5���ؽڽǶ�
% for HiddenUnitNum=3:MaxHiddenLayerNode %��������Ԫ�ĸ�������ȡ������ĺ�������
    fprintf('\n the hidden layer node');HiddenUnitNum
    TrainNNOut=[];
    TestNNOut=[];
    %for t=1:MaxRunningTime
        %fprintf('the current running times is');t
        [NewW1,NewB1,NewW2,NewB2]=PSO_GATrain(TrainSamIn,TrainSamOut,HiddenUnitNum);
        disp('PSO-GA�㷨ѵ�������������LM�㷨����ѵ�����硭��');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
net=newff(minmax(pn),[HiddenUnitNum 5],{'logsig','purelin'},'traingdm');
net.trainParam.show=10;
net.trainParam.lr=0.05; 
net.trainParam.epochs=5000;
%net.trainParam.epochs=20;
net.trainParam.goal=10^(-4);  
net.divideFcn = '';
NewW1
NewB1
NewW2
NewB2
net.iw{1,1}=NewW1;%7��6
net.b{1}=NewB1;%7��1
net.lw{2,1}=NewW2;%5��7
net.b{2}=NewB2;%5x1
tic
net=train(net,pn,tn);
toc%tic��toc���ڼ�¼ѵ��ʱ��
net.iw{1,1}
net.b{1}
net.lw{2,1}
net.b{2}
%�����������ݣ���������������������
Theta3=[30 40 50 60 70];
Theta3=Theta3/180*pi;                         %����ɻ���
%Theta2=[-40.0000 -40 50.0000 60 70.0000];
Theta4=[40 60 80 100 120];
Theta4=Theta4/180*pi;                         %����ɻ���
forward3=dofbot.fkine(Theta3)
forward4=dofbot.fkine(Theta4)
T3=forward1;
T4=forward2;
%��Ӧ�ؽڽ�
q3=dofbot.ikine(T3,[0 0 0 0 0],[1 1 1 1 1 0]);
q4=dofbot.ikine(T4,[0 0 0 0 0],[1 1 1 1 1 0]);
%��A�ƶ���B����ʱ6s��ÿ100ms����һ�ιؽڽ�
t=[0:0.03:6]';
%A�㵽B�㣬6���ؽڽ���6s�еı任���̡�v��a���ٶȺͼ��ٶȡ�
[qtest,v,a]=jtraj(q3,q4,t);
t1test=qtest(:,1);
t2test=qtest(:,2);
t3test=qtest(:,3);
t4test=qtest(:,4);
t5test=qtest(:,5);
Thetatest=[];
pxtest=[];
pytest=[];
pztest=[];
Txtest=[];
Tytest=[];
Tztest=[];
[Ntest,xxx]=size(qtest);%xxxΪ5
for i=1:Ntest
    theta1=t1test(i);
    theta2=t2test(i);
    theta3=t3test(i);
    theta4=t4test(i);
    theta5=t5test(i);
    Lie=[theta1
        theta2
        theta3
        theta4
        theta5];
    Thetatest=[Thetatest,Lie];
    T=eval(T05);
    pxtest=[pxtest
        T(1,4)];%Ŀ��������������
    pytest=[pytest
        T(2,4)];%Ŀ��������������
    pztest=[pztest
        T(3,4)];%Ŀ��������������
    R=T(1:3,1:3);
    r11=R(1,1);
    r12=R(1,2);
    r13=R(1,3);
    r21=R(2,1);
    r22=R(2,2);
    r23=R(2,3);
    r31=R(3,1);
    r32=R(3,2);
    r33=R(3,3);
    Tztest=[Tztest
        atan2(r21,r11)];%Ŀ��������������
    Tytest=[Tytest
        atan2(-r31,sqrt((r32^2)+(r33^2)))];%Ŀ��������������
    Txtest=[Txtest
        atan2(r32,r33)];%Ŀ��������������
end
%�����������ݾ���
INtest=[pxtest pytest pztest Txtest Tytest Tztest]';
%����Ŀ�����ݾ���
OUTtest=Thetatest;
ptest=INtest;
ttest=OUTtest;
[pntest,input_str] = mapminmax(ptest) ;
[tntest,output_str] = mapminmax(ttest) ;
% ����ѵ���õ����磬���ڲ������ݶ�BP�������
antest=sim(net,pntest);
% ���ú���mapminmax�ѷ���õ������ݻ�ԭΪԭʼ��������
% �°汾�Ƽ�ѵ��������һ���ͷ���һ����ʹ�� mapminmax ����
a = mapminmax('reverse',antest,output_str);
% a��ʵ����ttest�����������ݽ��в���
%��ͼ����ʾ�������ܣ�ָ��ѡ����������������������Ĳ�ֵ�ľ���ֵ����������
x=1:1:Ntest;
Etheta1=abs(a(1,:)-ttest(1,:));                                     
Etheta2=abs(a(2,:)-ttest(2,:)); 
Etheta3=abs(a(3,:)-ttest(3,:));
Etheta4=abs(a(4,:)-ttest(4,:));
Etheta5=abs(a(5,:)-ttest(5,:));
figure
% ttest=OUTtest;
% % [xxx1,TestSamNum]=size(INtest);
% % TestInEx=[INtest' ones(TestSamNum,1)]';
% % HiddenOut=logsig(W1Ex*TestInEx);
% % HiddenOutEx=[HiddenOut' ones(TestSamNum,1)]';
% % a=W2Ex*HiddenOutEx;
% x=[1:1:Ntest];
% Etheta1=abs(a(1,:)-ttest(1,:));                                     
% Etheta2=abs(a(2,:)-ttest(2,:)); 
% Etheta3=abs(a(3,:)-ttest(3,:));
% Etheta4=abs(a(4,:)-ttest(4,:));
% Etheta5=abs(a(5,:)-ttest(5,:));
% figure
subplot(2,1,1);plot(x,Etheta1,'r-o',x,Etheta2,'b--+',x,Etheta3,'y-.*') 
legend('�ؽڽǶ�1','�ؽڽǶ�2','�ؽڽǶ�3');
xlabel('����');ylabel('�������ֵ');
title('����������������˶�ѧ����������');
subplot(2,1,2);plot(x,Etheta4,'r-o',x,Etheta5,'b-.+')  
legend('�ؽڽǶ�4','�ؽڽǶ�5');
xlabel('����');ylabel('�������ֵ');
title('����������������˶�ѧ����������');
